﻿#include "eventinteraction.h"

#include <osgViewer/Viewer>                 // osgViewer::Viewer
#include "global.h"                         // MTNodeVistor
#include <osgUtil/IntersectionVisitor>      // Intersections::iterator
#include <osgUtil/LineSegmentIntersector>   // osgUtil::LineSegmentIntersector
#include <QDebug>

#include "simulatecontrolwidget.h"
#include "ifkinematicshandle.h"
EventInteraction::EventInteraction() :
    pViewer(nullptr),
    picked(nullptr),
    joint(BASE),
    axis(NONE),
    circle(EMPTY)
{

}


#include <QCursor>
bool EventInteraction::handle(const osgGA::GUIEventAdapter& guiEventAdapter, osgGA::GUIActionAdapter& guiActionAdapter)
{

    /// 获取场景viewer
    pViewer = dynamic_cast<osgViewer::Viewer*>(&guiActionAdapter);
    if (pViewer == nullptr) return false;

    int type = guiEventAdapter.getEventType();      /// 事件类型
    int key = guiEventAdapter.getKey();             /// 键盘按键
    int button = guiEventAdapter.getButton();       /// 鼠标按键

    /// 当鼠标左键按下
    if (type == osgGA::GUIEventAdapter::EventType::PUSH && button == osgGA::GUIEventAdapter::LEFT_MOUSE_BUTTON) {

        //pickAxis(guiEventAdapter.getX(), guiEventAdapter.getY());
        //pickJoint(guiEventAdapter.getX(), guiEventAdapter.getY());

        pick(guiEventAdapter.getX(), guiEventAdapter.getY());

        if (picked != nullptr) {

#ifndef NDEBUG
        qDebug() << QString(QStringLiteral("EventInteraction::handle: 选中关节: %1")).arg(joint);
        qDebug() << QString(QStringLiteral("EventInteraction::handle: 选中坐标轴: %1")).arg(axis);
        qDebug() << QString(QStringLiteral("EventInteraction::handle: 选中坐标环: %1")).arg(circle);
#endif

        lastWorldPoint = screen2World(guiEventAdapter.getX(), guiEventAdapter.getY());

        } else {

            picked = nullptr;
            axis = NONE;
            joint = BASE;
            circle = EMPTY;
        }
        return false;
    }

    if (picked && type == osgGA::GUIEventAdapter::EventType::DRAG) {

        //setCursor(Qt::ClosedHandCursor);

        /// 如果抓取的是坐标轴且不是环 则沿所选关节所选轴方向移动
        if (axis != NONE && circle == EMPTY) {
#ifndef NDEBUG
        //qDebug() << QString(QStringLiteral("EventInteraction::handle: 拖拽坐标轴"));
#endif
            axisTranslate(guiEventAdapter);
        }

        /// 如果抓取的是关节控制环则物体自旋
        if (circle != EMPTY) {
#ifndef NDEBUG
        //qDebug() << QString(QStringLiteral("EventInteraction::handle: 拖拽控制环"));
#endif
            rotateBySelf(guiEventAdapter);
        }

        /// 如果抓取的是关节
        if (axis == NONE && circle == EMPTY) {
#ifndef NDEBUG
        //qDebug() << QString(QStringLiteral("EventInteraction::handle: 拖拽关节"));
#endif
            rotateJoint(guiEventAdapter);
        }


        return true;

    }

    if (type == osgGA::GUIEventAdapter::EventType::RELEASE) {

        //setCursor(Qt::ArrowCursor);

        picked = nullptr;
        axis = NONE;
        joint = BASE;
        circle = EMPTY;
    }

#if 0
    switch (guiEventAdapter.getEventType()) {
        case osgGA::GUIEventAdapter::EventType::KEYDOWN: {
            int key = guiEventAdapter.getKey();
            if (key == osgGA::GUIEventAdapter::KEY_Alt_L) {
                altKey = true;
                break;
            } else {
                altKey = false;
            }
        }

#endif
    return false;   // 默认false 返回true则消息被截取，不会向下个事件处理类传递
}



static osg::Vec3 getCentorPoint(JointType jointType)
{
    osg::Vec3d center = {0,0,0};

    IFKinematicsHandle* ifKinehandle = IFKinematicsHandle::getInstance();
    double d1 = 0, a1 = 0, a2 = 0, a3 = 0, d4 = 0;
    ifKinehandle->getRobotParameter(d1, a1, a2, a3, d4);

    switch(jointType) {
        case J1:  center = {0, 0, d1};            break;
        case J2:  center = {a1,0, d1};            break;
        case J3:  center = {a1, 0, d1+a2};        break;
        case J4:  center = {a1+d4, 0, d1+a2+a3};  break;
        case J5:  center = {a1+d4, 0, d1+a2+a3};  break;
        case J6:  center = {a1+d4, 0, d1+a2+a3};  break;
        default: return {};
    }

    return center;
}


#include "Eigen/Dense"
static const double pi = 3.1415926;

static float getAngleBetweenVector(osg::Vec3d vec1, osg::Vec3d vec2)
{
    Eigen::Vector3d v1 = {vec1.x(), vec1.y(), vec1.z()};
    Eigen::Vector3d v2 = {vec2.x(), vec2.y(), vec2.z()};
    double radian_angle = atan2(v1.cross(v2).norm(), v1.transpose() * v2);

    radian_angle *= (vec1.x() - vec2.x() > 0 ? 1 : -1);


#ifndef NDEBUG
    qDebug() << "[getAngleBetweenVector]" << radian_angle * 180 / pi;
#endif

    return radian_angle * 180 / pi;
}




static bool isMeetLimit(Joint curJointValue)
{
    IFKinematicsHandle* ifKinehandle = IFKinematicsHandle::getInstance();

    /// 获得各关节最大最小值
    QVector<float> Joint_MIN; QVector<float> Joint_MAX;
    ifKinehandle->getRobotJointLimit(Joint_MIN, Joint_MAX);

    QVector<double> jointVal = {
        curJointValue.j1,
        curJointValue.j2,
        curJointValue.j3,
        curJointValue.j4,
        curJointValue.j5,
        curJointValue.j6,
    };

    for (int i = 0; i < 6 ; ++i) {
        if (jointVal[i] < Joint_MIN[i] || jointVal[i] > Joint_MAX[i]) {
#ifndef NDEBUG
        qDebug() << QString("[isMeetLimit] J%1 out of limit").arg(i+1);
#endif
            return true;
        }
    }

    return false;
}






/// 鼠标旋转关节
void EventInteraction::rotateJoint(const osgGA::GUIEventAdapter& guiEventAdapter)
{
    if (!picked) return;

#if 1
    /// 计算世界系下当前鼠标选中点
    osg::Vec3 curWorldPoint = screen2World(guiEventAdapter.getX(), guiEventAdapter.getY());
#if 0
    /// 计算两个向量家教
    osg::Vec3 centerWorldPoint = getCentorPoint(joint);
    osg::Vec3 v1 = curWorldPoint - centerWorldPoint;
    osg::Vec3 v2 = lastWorldPoint - centerWorldPoint;
    float angle =  getAngleBetweenVector(v1, v2);
#endif
    float angle = (curWorldPoint.x()-lastWorldPoint.x() > 0) ? 1 : -1;
    lastWorldPoint = curWorldPoint;
#endif

    osg::Matrix curMatrix = picked->getMatrix();
    osg::Vec3d offset = getOffset(joint);

    rotateBySelf(joint, curMatrix, offset, angle);
    picked->setMatrix(curMatrix);

}


osg::Vec3d EventInteraction::getOffset(const JointType& joint)
{

    osg::Vec3d offset = {0, 0, 0};
    IFKinematicsHandle* ifKinehandle = IFKinematicsHandle::getInstance();
    double d1, a1, a2, a3, d4;
    ifKinehandle->getRobotParameter(d1, a1, a2, a3, d4);

    switch(joint) {
        case BASE:
            offset = {0, 0, 0};
            break;
        case J1:
            offset = {0, 0, d1};
            break;
        case J2:
            offset = {a1,0, d1};
            break;
        case J3:
            offset = {a1, 0, d1+a2};
            break;
        case J4:
            offset = {a1+d4, 0, d1+a2+a3};
            break;
        case J5:
            offset = {a1+d4, 0, d1+a2+a3};
            break;
        case J6:
            offset = {a1+d4, 0, d1+a2+a3};
            break;
        default:
            offset = {0 ,0 ,0};
    }

    return offset;

}


void EventInteraction::rotateBySelf(const JointType& joint, osg::Matrix& curMatrix, const osg::Vec3d& offset, const float& angle)
{
    /// 获取当前各轴角度
    simulateControlWidget = SimulateControlWidget::getInstance();
    if (!simulateControlWidget) return;
    Joint curJointValue = simulateControlWidget->getCurJointsAngle();

    switch(joint) {
        case J1:
            curJointValue.j1 += angle;
            break;
        case J2:
            curJointValue.j2 += angle;
            break;
        case J3:
            curJointValue.j3 += angle;
            break;
        case J4:
            curJointValue.j4 += angle;
            break;
        case J5:
            curJointValue.j5 += angle;
            break;
        case J6:
            curJointValue.j6 += angle;
            break;
        default:
            return;
    }

    /// 判断是否超限,超限就return
    if (isMeetLimit(curJointValue)) {
#ifndef NDEBUG
        //qDebug() << "[EventInteraction::rotateJoint] out of limit";
#endif
        return;
    }

    /// 旋转前获取各关节角度
    curMatrix *= osg::Matrix::translate(-offset);
    switch(joint) {
        case J1:
            curMatrix *= osg::Matrix::rotate(osg::inDegrees(angle), osg::Z_AXIS);
            break;
        case J2:
            curMatrix *= osg::Matrix::rotate(osg::inDegrees(angle), osg::Y_AXIS);
            break;
        case J3:
            curMatrix *= osg::Matrix::rotate(osg::inDegrees(angle), osg::Y_AXIS);
            break;
        case J4:
            curMatrix *= osg::Matrix::rotate(osg::inDegrees(angle), osg::X_AXIS);
            break;
        case J5:
            curMatrix *= osg::Matrix::rotate(osg::inDegrees(angle), osg::Y_AXIS);
            break;
        case J6:
            curMatrix *= osg::Matrix::rotate(osg::inDegrees(angle), osg::Z_AXIS);
            break;
        default:
            return;
    }
    curMatrix *= osg::Matrix::translate(offset);

    simulateControlWidget->setCurJointsAngle(curJointValue);

}


void EventInteraction::axisTranslate(const osgGA::GUIEventAdapter& guiEventAdapter)
{
    if (!picked) return;

    // 将两个屏幕中的点转换成世界坐标系下的两个点,计算世界系下两点的距离
    osg::Vec3 curWorldPoint = screen2World(guiEventAdapter.getX(), guiEventAdapter.getY());
    float dx =  curWorldPoint.x() - lastWorldPoint.x();
    float dy =  curWorldPoint.y() - lastWorldPoint.y();
    float dz =  curWorldPoint.z() - lastWorldPoint.z();
    lastWorldPoint = curWorldPoint;

    osg::Matrix curMatrix = picked->getMatrix();
    switch(axis) {
    case X:
        curMatrix = osg::Matrix::translate(dx, 0, 0) * curMatrix;
        break;
    case Y:
        curMatrix = osg::Matrix::translate(0, dy, 0) * curMatrix;
        break;
    case Z:
        curMatrix = osg::Matrix::translate(0, 0, dz) * curMatrix;
        break;
    default:
        return;
    }
    picked->setMatrix(curMatrix);
}


/* 单轴旋转
 * @joint 要旋转的关节
 * @curMatrix 当前关节矩阵
 * @offset 关节轴距原点偏移量
 * @angle 旋转角度
*/
void EventInteraction::rotateBySelf(const osgGA::GUIEventAdapter& guiEventAdapter)
{
    if (!picked) return;

    // 计算世界系下两个点, 绕xyz轴的度数
    osg::Vec3 curWorldPoint = screen2World(guiEventAdapter.getX(), guiEventAdapter.getY());
    float angle =  curWorldPoint.x() - lastWorldPoint.x() > 0 ? 5 : -5;
    lastWorldPoint = curWorldPoint;

    osg::Matrix curMatrix = picked->getMatrix();
    float redia = osg::DegreesToRadians(angle);
    switch(circle) {
    case CIRCLE_X:
        curMatrix = osg::Matrix::rotate(redia, osg::X_AXIS) * curMatrix;
        break;
    case CIRCLE_Y:
        curMatrix = osg::Matrix::rotate(redia, osg::Y_AXIS) * curMatrix;
        break;
    case CIRCLE_Z:
        curMatrix = osg::Matrix::rotate(redia, osg::Z_AXIS) * curMatrix;
        break;
    default:
        return;
    }
    picked->setMatrix(curMatrix);
}



/* 屏幕坐标转世界坐标
 * @pViewer 场景指针
 * @x, y 屏幕上x y坐标
 * @return 世界坐标系下的x,z坐标
 */
osg::Vec3 EventInteraction::screen2World(float x, float y)
{
    // 屏幕坐标系下的点
    osg::Vec3 vScreen(x, y, 0);
    osg::ref_ptr<osg::Camera> camera = pViewer->getCamera();
    // 获取世界到屏幕的转换矩阵
    osg::Matrix mVPW = camera->getViewMatrix() * camera->getProjectionMatrix() * camera->getViewport()->computeWindowMatrix();
    osg::Matrix invertVPW;
    invertVPW.invert(mVPW);
    // 屏幕坐标系右乘转换矩阵的逆得到世界坐标系
    return vScreen * invertVPW;
}


/* 抓取物体,获得物体的转换节点
 * @pViewer 场景指针
 * @x, y 屏幕上x y坐标
*/
void EventInteraction::pickJoint(float x, float y)
{
#ifndef NDEBUG
        qDebug() << QStringLiteral("EventInteraction::pickJoint") << x << y;
#endif
    // 定义一个向里射的射线
    osgUtil::LineSegmentIntersector::Intersections intersections;
    // 计算射线碰撞检测结果
    if (pViewer->computeIntersections(x, y, intersections)) {
        // 碰撞的第一个节点
        osgUtil::LineSegmentIntersector::Intersections::iterator inter = intersections.begin();
        // 获得这个节点所有的继承关系
        const osg::NodePath& nodePath = inter->nodePath;
        // 遍历节点路径找到这个节点的矩阵节点
        for (int i = nodePath.size() - 1; i >= 0; --i) {
            // 动态转换失败会返回null,帮助我们找到想要的类型
             osg::MatrixTransform* mt = dynamic_cast<osg::MatrixTransform*>(nodePath[i]);
             if (mt == nullptr) continue;
             joint = static_cast<JointType>(mt->getName()[0] - '0');
             picked = mt;
             circle = EMPTY;
             axis = NONE;
             return;
        }
    }
    picked = nullptr;
}



/** 抓取坐标轴,获取所属物体的转换节点
 * @pViewer 场景指针
 * @x, y 屏幕上x y坐标
*/
void EventInteraction::pickAxis(float x, float y)
{

#ifndef NDEBUG
        qDebug() << QStringLiteral("EventInteraction::pickAxis") << x << y;
#endif

#if 1
    /// 定义一个向里射的多面体求交器,能够识别点线碰撞检测
    osg::ref_ptr<osgUtil::PolytopeIntersector> picker = new osgUtil::PolytopeIntersector(osgUtil::Intersector::WINDOW, x-5.0, y-5.0, x+5.0, y+5.0);
    osgUtil::IntersectionVisitor iv(picker.get());
    pViewer->getCamera()->accept(iv);

    if (picker->containsIntersections()) {

        osgUtil::PolytopeIntersector::Intersections setIntersections = picker->getIntersections();
        osgUtil::PolytopeIntersector::Intersection intersection = *setIntersections.begin();
        const osg::NodePath& nodePath = intersection.nodePath;
#else

    /// 定义一个向里射的线段求交器
    osgUtil::LineSegmentIntersector::Intersections intersections;
    /// 计算射线碰撞检测结果
    if (pViewer->computeIntersections(x, y, intersections)) {
        /// 碰撞的第一个节点
        osgUtil::LineSegmentIntersector::Intersections::iterator inter = intersections.begin();
        /// 获得这个节点所有的继承关系
        const osg::NodePath& nodePath = inter->nodePath;

#endif

        /// 是否是关节转换矩阵节点
        bool isJointMatrixTransform = false;

        /// 遍历节点路径找到这个节点的矩阵节点
        for (int i = nodePath.size() - 1; i >= 0; --i) {
            /// 动态转换失败会返回null,帮助我们找到想要的类型
            /// 因为circle属于Geode类型,因此如果发现点中的物体是Geode类型,说明点到的是circle上
            osg::Geode* pickedCircle = dynamic_cast<osg::Geode*>(nodePath[i]);

            if (pickedCircle) {
                circle = static_cast<Circle>(pickedCircle->getName()[0]-'0');
            }

            osg::MatrixTransform* mt = dynamic_cast<osg::MatrixTransform*>(nodePath[i]);

            if (mt == nullptr) continue;

            /// 确定坐标系属于哪个关节, 第一个转换节点是xyz坐标轴某一轴的转换矩阵
            if (mt && !isJointMatrixTransform) {
                axis = static_cast<Axis>(mt->getName()[0]-'0');
                isJointMatrixTransform = true;
                continue;

            }

            /// 获取所属关节的转换节点
            joint = static_cast<JointType>(mt->getName()[0] - '0');
            picked = mt;
            return;
        }
    }
    picked = nullptr;

}


void EventInteraction::pick(float x, float y)
{

#ifndef NDEBUG
        qDebug() << QStringLiteral("EventInteraction::pick") << x << y;
#endif

    /// 定义一个向里射的多面体求交器,能够识别点线碰撞检测
    osg::ref_ptr<osgUtil::PolytopeIntersector> picker = new osgUtil::PolytopeIntersector(osgUtil::Intersector::WINDOW, x-5.0, y-5.0, x+5.0, y+5.0);
    osgUtil::IntersectionVisitor iv(picker.get());
    pViewer->getCamera()->accept(iv);

    if (picker->containsIntersections()) {

        osgUtil::PolytopeIntersector::Intersections setIntersections = picker->getIntersections();
        osgUtil::PolytopeIntersector::Intersection intersection = *setIntersections.begin();
        const osg::NodePath& nodePath = intersection.nodePath;

        /// 判断碰撞的是关节还是物体的控制环
        bool isPickJoint = true;

        /// 遍历节点路径找到这个节点的矩阵节点
        for (int i = nodePath.size() - 1; i >= 0; --i) {
            /// 动态转换失败会返回null,帮助我们找到想要的类型
            /// 因为circle属于Geode类型,因此如果发现点中的物体是Geode类型,说明点到的是circle上
            osg::Geode* pickedCircle = dynamic_cast<osg::Geode*>(nodePath[i]);

            if (pickedCircle) {
                circle = static_cast<Circle>(pickedCircle->getName()[0]-'0');
                //isPickJoint = false;
                break;
            }
        }

        isPickJoint ? pickJoint(x, y) : pickAxis(x, y);

    }
}



